#ifndef CAN_RECEIVE_H
#define CAN_RECEIVE_H

#include "main.h"
#include "arm_math.h"

#define CHASSIS_CAN		hcan1
#define GIMBAL_CAN		hcan2
#define SHOOT_CAN		hcan2

#define MIT_MODE		0x000

#define P_MIN			-PI
#define P_MAX			PI
#define V_MIN			-30.0f
#define V_MAX			30.0f
#define KP_MIN			0.0f
#define KP_MAX			500.0f
#define KD_MIN			0.0f
#define KD_MAX			5.0f
#define T_MIN			-10.0f
#define T_MAX			10.0f

typedef struct 
{
	uint8_t id;
	uint16_t mode;
	uint8_t err_state;
	int16_t p_int;
	int16_t v_int;
	int16_t t_int;
	int16_t kp_int;
	int16_t kd_int;
	fp32 pos;
	fp32 vel;
	fp32 tor;
	fp32 Kp;
	fp32 Kd;
	fp32 Temp;
} mit_motor_measure_t;

typedef enum
{
    CAN_CHASSIS_ALL_ID = 0x200,
    CAN_3508_M1_ID = 0x201,
    CAN_3508_M2_ID = 0x202,
    CAN_3508_M3_ID = 0x203,
    CAN_3508_M4_ID = 0x204,

    CAN_GIMBAL_ALL_ID = 0x1FF,
    CAN_YAW_MOTOR_ID = 0x205,
    CAN_PIT_MOTOR_ID = 0x206,
	CAN_TRIGER_MOTOR_ID = 0x207,

	CAN_SHOOT_ALL_ID = 0x200,
    CAN_FRIC1_ID = 0x201,
    CAN_FRIC2_ID = 0x202,

    CAN_SUPERCAP_TX_ID = 0x210,
    CAN_SUPERCAP_RX_ID = 0x211,
} can_msg_id_e;

typedef struct
{
    uint16_t ecd;
    int16_t speed_rpm;
    int16_t given_current;
    uint8_t temperate;
    int16_t last_ecd;
} motor_measure_t;

typedef struct
{
    uint16_t battery_voltage;
    int16_t chassis_power;
    int16_t given_current;
    uint8_t capacitance_percentage;
    uint8_t power_set;
} sup_measure_t;

extern void CAN_cmd_gimbal(int16_t yaw, int16_t pitch, int16_t fric, int16_t rev);
extern void CAN_cmd_chassis_reset_ID(void);
extern void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4);
extern void CAN_cmd_shoot(int16_t shoot1 ,int16_t shoot2, int16_t rev1, int16_t rev2);
extern void CAN_cmd_supercap(int16_t power_set);

extern const motor_measure_t *get_yaw_gimbal_motor_measure_point(void);
extern const motor_measure_t *get_pitch_gimbal_motor_measure_point(void);
extern const motor_measure_t *get_trigger_motor_measure_point(void);
extern const motor_measure_t *get_chassis_motor_measure_point(uint8_t i);
extern const motor_measure_t *get_fric1_motor_measure_point(void);
extern const motor_measure_t *get_fric2_motor_measure_point(void);
extern const sup_measure_t *get_supercap_measure_point(void);

extern void enable_radar_yaw_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id);
extern void init_radar_yaw_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id);
extern void disable_radar_yaw_motor_mode(CAN_HandleTypeDef *hcan, uint16_t motor_id, uint16_t mode_id);
extern void radar_yaw_motor_init(void);
extern void mit_ctrl_big_yaw_motor(CAN_HandleTypeDef* hcan, uint16_t motor_id, fp32 pos, fp32 vel, fp32 kp, fp32 kd, fp32 torq);
extern const mit_motor_measure_t *get_yaw_gimbal_mit_motor_measure_point(void);

#endif
